// --------------------------------------------------------------------------------------------------------------------
// <copyright file="JointControllerDisplayView.xaml.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Visualization
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using System.Windows;
    using System.Windows.Controls;
    using System.Windows.Data;
    using System.Windows.Documents;
    using System.Windows.Input;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;
    using System.Windows.Navigation;
    using System.Windows.Shapes;

    /// <summary>
    /// Interaction logic for JointControllerDisplayView.xaml
    /// </summary>
    public partial class JointControllerDisplayView : UserControl
    {
        /// <summary>
        /// Joint position sliders.
        /// </summary>
        private Slider[] jointPositionSliders;

        /// <summary>
        /// Joint position text boxes.
        /// </summary>
        private TextBox[] jointPositionTextBoxes;

        /// <summary>
        /// Text boxes for P gains
        /// </summary>
        private TextBox[] pTextBoxes;

        /// <summary>
        /// Text boxes for D gains
        /// </summary>
        private TextBox[] dTextBoxes;

        /// <summary>
        /// Text boxes for deadband
        /// </summary>
        private TextBox[] deadbandTextBoxes;

        /// <summary>
        /// Text boxes for max delta
        /// </summary>
        private TextBox[] maxDeltaTextBoxes;

        /// <summary>
        /// Initializes a new instance of the <see cref="JointControllerDisplayView"/> class.
        /// </summary>
        public JointControllerDisplayView()
        {
            this.InitializeComponent();

            this.jointPositionSliders = new Slider[]
            {
                this.JointPositionSlider0,
                this.JointPositionSlider1,
                this.JointPositionSlider2,
                this.JointPositionSlider3,
                this.JointPositionSlider4,
                this.JointPositionSlider5,
                this.JointPositionSlider6
            };

            this.jointPositionTextBoxes = new TextBox[]
            {
                this.JointPositionTextBox0,
                this.JointPositionTextBox1,
                this.JointPositionTextBox2,
                this.JointPositionTextBox3,
                this.JointPositionTextBox4,
                this.JointPositionTextBox5,
                this.JointPositionTextBox6
            };

            this.pTextBoxes = new TextBox[]
            {
                this.P0TextBox,
                this.P1TextBox,
                this.P2TextBox,
                this.P3TextBox,
                this.P4TextBox,
                this.P5TextBox,
                this.P6TextBox
            };

            this.dTextBoxes = new TextBox[]
            {
                this.D0TextBox,
                this.D1TextBox,
                this.D2TextBox,
                this.D3TextBox,
                this.D4TextBox,
                this.D5TextBox,
                this.D6TextBox
            };

            this.deadbandTextBoxes = new TextBox[]
            {
                this.Deadband0TextBox,
                this.Deadband1TextBox,
                this.Deadband2TextBox,
                this.Deadband3TextBox,
                this.Deadband4TextBox,
                this.Deadband5TextBox,
                this.Deadband6TextBox
            };

            this.maxDeltaTextBoxes = new TextBox[]
            {
                this.MaxDelta0TextBox,
                this.MaxDelta1TextBox,
                this.MaxDelta2TextBox,
                this.MaxDelta3TextBox,
                this.MaxDelta4TextBox,
                this.MaxDelta5TextBox,
                this.MaxDelta6TextBox
            };
        }

        /// <summary>
        /// Handle updates to joint position slider 0
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">The parameter is not used.</param>
        private void JointPositionSlider_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
        {
            int index = Convert.ToInt32((string)(((Slider)e.Source).Tag));
            this.OnSliderValueChange(index);
        }

        /// <summary>
        /// Update a slider value
        /// </summary>
        /// <param name="index">Index of slider to update </param>
        private void OnSliderValueChange(int index)
        {
            string textUpdate = this.jointPositionSliders[index].Value.ToString("F3");
            this.jointPositionTextBoxes[index].Text = textUpdate;
            this.SetJointPositions();
        }

        /// <summary>
        /// Update textbox value
        /// </summary>
        /// <param name="index">An integer value determining an index.</param>
        private void OnTextBoxUpdate(int index)
        {
            this.jointPositionSliders[index].Value = Convert.ToDouble(this.jointPositionTextBoxes[index].Text);
        }

        /// <summary>
        /// Handle joint position lost focus event.
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">Event args.</param>
        private void JointPositionTextBox_LostFocus(object sender, RoutedEventArgs e)
        {
            int index = Convert.ToInt32((string)(((TextBox)e.Source).Tag));
            this.OnTextBoxUpdate(index);
        }

        /// <summary>
        /// Handle joint position key down event.
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">Event args.</param>
        private void JointPositionTextBox_KeyDown(object sender, KeyEventArgs e)
        {
            int index = Convert.ToInt32((string)(((TextBox)e.Source).Tag));
            if (e.Key == Key.Return)
            {
                this.OnTextBoxUpdate(index);
            }
        }

        /// <summary>
        /// Sends joint positions to manipulator agent
        /// </summary>
        private void SetJointPositions()
        {
            JointControllerDisplayModel model = (JointControllerDisplayModel)this.DataContext;
            for (int i = 0; i < this.jointPositionSliders.Length; ++i)
            {
                model.JointCommandedPosition[i] = this.jointPositionSliders[i].Value;
            }
        }

        /// <summary>
        /// Update joint positions from actual joint positions
        /// </summary>
        private void UpdateJointPositionsFromActual()
        {
            JointControllerDisplayModel model = (JointControllerDisplayModel)this.DataContext;
            for (int i = 0; i < this.jointPositionSliders.Length; ++i)
            {
                this.jointPositionSliders[i].Value = model.JointActualPosition[i];
            }
        }

        /// <summary>
        /// Handle update button press for updating joint positions from actual joint positions
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">The parameter is not used.</param>
        private void UpdateJointPositionsFromActual(object sender, RoutedEventArgs e)
        {
            this.UpdateJointPositionsFromActual();
        }

        /// <summary>
        /// Handle press of a 'Set' button for PD gains
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">Routed event args.</param>
        private void SetButtonClick(object sender, RoutedEventArgs e)
        {
            int index = Convert.ToInt32((string)(((Button)e.Source).Tag));

            JointControllerDisplayModel model = (JointControllerDisplayModel)this.DataContext;
            model.PGains[index] = Convert.ToDouble(this.pTextBoxes[index].Text);
            model.DGains[index] = Convert.ToDouble(this.dTextBoxes[index].Text);
            model.Deadband[index] = Convert.ToDouble(this.deadbandTextBoxes[index].Text);
            model.MaxDelta[index] = Convert.ToDouble(this.maxDeltaTextBoxes[index].Text);
            model.PDGainsToUpdate.Enqueue(index);
        }
    }
}
